#include "Utilities.h"
#include "Trajectory.h"
#include "Trajectories.h"

using namespace utilities;

using std::cout;
using std::endl;
using std::cerr;
using std::string;
using std::vector;
using std::stringstream;
using std::ifstream;
using std::ofstream;

_TRAJECTORY_BEGIN_

void Trajectories::readFromFile(const string& filename, int nameIndex, int startDimension, int endDimension)
{
	cout << "Begin to read.\n";
	int dimension = endDimension - startDimension + 1;

	ifstream in;
	in.open(filename.c_str());
	if (NULL == in)
	{
		cerr << "File not exists!\n";
		exit(0);
	}
	string tmpLine;
	vector<string> tmpTokens;
	string lastName("");
	vector<string> trajNames;


	while(!in.eof())
	{
		getline(in, tmpLine, '\n');
		tmpTokens = utilities::Split(tmpLine, '\t');
		if(tmpTokens.empty())
			continue;
		DataPoint point;
		point.resize(dimension);
		for(int i = 0; i < dimension; i++)
		{
			istringstream iss(tmpTokens[startDimension + i]);
			double dValue;
			iss >> dValue;
			point[i] = dValue;
		}
		if(lastName != tmpTokens[nameIndex])	// A new trajectory found
		{
			Trajectory traj;
			lastName = tmpTokens[nameIndex];
			trajNames.push_back(lastName);
			m_Trajs.push_back(traj);
		}
		m_Trajs[m_Trajs.size() - 1].addPoint(point);
	}

	cout << "There are " << m_Trajs.size() << " trajectories.\n";
	for(int i = 0; i < (int)m_Trajs.size(); i++)
	{
		m_Trajs[i].setName(trajNames[i]);
	}

	cout << "End of read.\n";

}

/************************************************************************/
/*	The lower bound of the distance.                                    */
/*	Parameters:															*/
/*		CPi		- the coarse partition of the i-th trajectory.			*/
/*		CPk		- the coarse partition of the k-th trajectory.			*/
/*		index_i - the index of the i-th trajectory						*/
/*		index_j - the index of the j-th coarse partition				*/
/*		index_k - the index of the k-th trajectory						*/
/*		index_l - the index of the l-th coarse partition				*/
/************************************************************************/
double Trajectories::lb(const std::vector<int> &CPi, const std::vector<int> &CPk, int index_i, int index_j, int index_k, int index_l)
{

	double lb = 0.0;
	double maxPer_j = 0.0, maxPer_l = 0.0;
	double maxLen_j = 0.0, maxLen_l = 0.0;
	double minLen_j = 0.0, minLen_l = 0.0;
	double maxThe_j = 0.0, maxThe_l = 0.0;
	m_Trajs[index_i].getCoarseParameters(index_j, maxPer_j, maxLen_j, minLen_j, maxThe_j);
	m_Trajs[index_k].getCoarseParameters(index_l, maxPer_l, maxLen_l, minLen_l, maxThe_l);
	const vector<int> &coaPk = CPk;
	const vector<int> &coaPi = CPi;

	//	Calculate the lb(Li, lj, d+)	
	pair<double, double> lineParak = calculateLineParameters(m_Trajs[index_k][coaPk[index_l]][0], m_Trajs[index_k][coaPk[index_l]][1],
		m_Trajs[index_k][coaPk[index_l + 1]][0], m_Trajs[index_k][coaPk[index_l + 1]][1]);
	pair<double, double> lineParai = calculateLineParameters(m_Trajs[index_i][coaPi[index_j]][0], m_Trajs[index_i][coaPi[index_j]][1],
		m_Trajs[index_i][coaPi[index_j + 1]][0], m_Trajs[index_i][coaPi[index_j + 1]][1]);

	double perL1 = calculateLperpendicular(m_Trajs[index_i][coaPi[index_j]][0], m_Trajs[index_i][coaPi[index_j]][1], lineParak);
	double perL2 = calculateLperpendicular(m_Trajs[index_i][coaPi[index_j + 1]][0], m_Trajs[index_i][coaPi[index_j + 1]][1], lineParak);
	double dlbDper = min(perL1, perL2) - (maxPer_j + maxPer_l);

	//	Calculate the lb(Li, Lj, d||)	
	double dlbDpar;
	if (lineParak.first == lineParai.first)	
	{
		dlbDpar = 0;
	} 
	else
	{
		dlbDpar = calculateParallelDistanceEx(m_Trajs[index_k][coaPk[index_l]][0], m_Trajs[index_k][coaPk[index_l]][1],
			m_Trajs[index_k][coaPk[index_l + 1]][0], m_Trajs[index_k][coaPk[index_l + 1]][1],
			m_Trajs[index_i][coaPi[index_j]][0], m_Trajs[index_i][coaPi[index_j]][1],
			m_Trajs[index_i][coaPi[index_j + 1]][0], m_Trajs[index_i][coaPi[index_j + 1]][1]);
	}

	//	Calculate the lb(Li, Lj, d@)	
	double theta = atan(lineParai.first) - atan(lineParak.first);
	double dlbDthe = min(minLen_j, minLen_l) + sin(theta - maxThe_j - maxThe_l);
	lb = m_Wper * dlbDper + m_Wpar * dlbDpar + m_Wthe * dlbDthe;

	return lb;
}

/************************************************************************/
/*	The upper bound of the distance.                                    */
/*	Parameters:															*/
/*		CPi		- the coarse partition of the i-th trajectory.			*/
/*		CPk		- the coarse partition of the k-th trajectory.			*/
/*		index_i - the index of the i-th trajectory						*/
/*		index_j - the index of the j-th coarse partition				*/
/*		index_k - the index of the k-th trajectory						*/
/*		index_l - the index of the l-th coarse partition				*/
/************************************************************************/
double Trajectories::ub(const std::vector<int> &CPi, const std::vector<int> &CPk, int index_i, int index_j, int index_k, int index_l)
{
	double ub = 0.0;

	double maxPer_j, maxPer_l;
	double maxLen_j, maxLen_l;
	double minLen_j, minLen_l;
	double maxThe_j, maxThe_l;
	m_Trajs[index_i].getCoarseParameters(index_j, maxPer_j, maxLen_j, minLen_j, maxThe_j);
	m_Trajs[index_k].getCoarseParameters(index_l, maxPer_l, maxLen_l, minLen_l, maxThe_l);
	const vector<int> &coaPk = CPk;
	const vector<int> &coaPi = CPi;

	/*	Calculate the lb(Li, lj, d+)	*/
	pair<double, double> lineParak = calculateLineParameters(m_Trajs[index_k][coaPk[index_l]][0], m_Trajs[index_k][coaPk[index_l]][1],
		m_Trajs[index_k][coaPk[index_l + 1]][0], m_Trajs[index_k][coaPk[index_l + 1]][1]);
	pair<double, double> lineParai = calculateLineParameters(m_Trajs[index_i][coaPi[index_j]][0], m_Trajs[index_i][coaPi[index_j]][1],
		m_Trajs[index_i][coaPi[index_j + 1]][0], m_Trajs[index_i][coaPi[index_j + 1]][1]);

	double perL1 = calculateLperpendicular(m_Trajs[index_i][coaPi[index_j]][0], m_Trajs[index_i][coaPi[index_j]][1], lineParak);
	double perL2 = calculateLperpendicular(m_Trajs[index_i][coaPi[index_j + 1]][0], m_Trajs[index_i][coaPi[index_j + 1]][1], lineParak);
	double dubDper = min(perL1, perL2) + (maxPer_j + maxPer_l);

	/*	Calculate the lb(Li, Lj, d||)	*/
	double dubDpar;
	if (lineParak.first == lineParai.first)	
	{
		if (   m_Trajs[index_i][coaPi[index_j]][0] > m_Trajs[index_k][coaPk[index_l]][0] && m_Trajs[index_i][coaPi[index_j + 1]][0] < m_Trajs[index_k][coaPk[index_l + 1]][0]
		|| m_Trajs[index_i][coaPi[index_j]][0] < m_Trajs[index_k][coaPk[index_l]][0] && m_Trajs[index_i][coaPi[index_j + 1]][0] > m_Trajs[index_k][coaPk[index_l + 1]][0]
		|| m_Trajs[index_i][coaPi[index_j]][1] > m_Trajs[index_k][coaPk[index_l]][1] && m_Trajs[index_i][coaPi[index_j + 1]][1] < m_Trajs[index_k][coaPk[index_l + 1]][1]
		|| m_Trajs[index_i][coaPi[index_j]][1] < m_Trajs[index_k][coaPk[index_l]][1] && m_Trajs[index_i][coaPi[index_j + 1]][1] > m_Trajs[index_k][coaPk[index_l + 1]][1])
		{
			dubDpar = max(maxLen_j, maxLen_l);
		}
		else
			dubDpar = calculateEuclideanDistance(m_Trajs[index_k][coaPk[index_l]][0], m_Trajs[index_k][coaPk[index_l]][1],
			m_Trajs[index_k][coaPk[index_l + 1]][0], m_Trajs[index_k][coaPk[index_l + 1]][1]) 
			+ calculateEuclideanDistance(m_Trajs[index_i][coaPi[index_j]][0], m_Trajs[index_i][coaPi[index_j]][1],
			m_Trajs[index_i][coaPi[index_j + 1]][0], m_Trajs[index_i][coaPi[index_j + 1]][1]) 
			- calculateParallelDistanceEx(m_Trajs[index_k][coaPk[index_l]][0], m_Trajs[index_k][coaPk[index_l]][1],
			m_Trajs[index_k][coaPk[index_l + 1]][0], m_Trajs[index_k][coaPk[index_l + 1]][1],
			m_Trajs[index_i][coaPi[index_j]][0], m_Trajs[index_i][coaPi[index_j]][1],
			m_Trajs[index_i][coaPi[index_j + 1]][0], m_Trajs[index_i][coaPi[index_j + 1]][1]);
	} 
	else
	{
		dubDpar = calculateEuclideanDistance(m_Trajs[index_k][coaPk[index_l]][0], m_Trajs[index_k][coaPk[index_l]][1],
			m_Trajs[index_k][coaPk[index_l + 1]][0], m_Trajs[index_k][coaPk[index_l + 1]][1]) 
			+ calculateEuclideanDistance(m_Trajs[index_i][coaPi[index_j]][0], m_Trajs[index_i][coaPi[index_j]][1],
			m_Trajs[index_i][coaPi[index_j + 1]][0], m_Trajs[index_i][coaPi[index_j + 1]][1]) 
			+ calculateParallelDistanceEx(m_Trajs[index_k][coaPk[index_l]][0], m_Trajs[index_k][coaPk[index_l]][1],
			m_Trajs[index_k][coaPk[index_l + 1]][0], m_Trajs[index_k][coaPk[index_l + 1]][1],
			m_Trajs[index_i][coaPi[index_j]][0], m_Trajs[index_i][coaPi[index_j]][1],
			m_Trajs[index_i][coaPi[index_j + 1]][0], m_Trajs[index_i][coaPi[index_j + 1]][1]);
	}

	//	Calculate the lb(Li, Lj, d@)	
	double theta = atan(lineParai.first) - atan(lineParak.first);
	double dubDthe = min(maxLen_j, maxLen_l) + sin(theta + maxThe_j + maxThe_l);
	ub = m_Wper * dubDper + m_Wpar * dubDpar + m_Wthe * dubDthe;

	return ub;
}

/************************************************************************/
/*	The member function of the class Trajectories.                      */
/************************************************************************/
void Trajectories::findOutlier()
{

	long startTime = (long)clock();

	vector<vector<int>> CP;
	calcCP(*&CP);

	vector<vector<CL>> CLset;
	vector<vector<CTR>> CTRset;
	calcCLset(*&CLset, *&CTRset);

	
	//calcCTRset(*&CTRset, *&CLset);

	CLset.clear();
	vector<vector<CL>>(CLset).swap(CLset);

	vector<vector<double>> adj;
	calcadj(*&adj);

	CP.clear();
	vector<vector<int>>(CP).swap(CP);

	vector<vector<int>> OP;

	calcOP(*&OP, *&CTRset, *&adj);

	calcOT(*&OP);
	OP.clear();
	vector<vector<int>> (OP).swap(OP);

	long endTime = (long)clock();

	costTime = (double)(endTime - startTime) / CLOCKS_PER_SEC;

	cout << "Time cost:" << costTime << endl;

}

void Trajectories::calcCP(vector<vector<int>> &CP)
{
	/*	Calculate the coarse partition	*/
	CP.reserve(m_Trajs.size());
	for (int i = 0; i < (int)m_Trajs.size(); ++i)
	{
		const vector<int>& CPi = m_Trajs[i].coarsePartition();  
		CP.push_back(CPi);
	}

	for (int i = 0; i < (int)m_Trajs.size(); ++i)
	{
		m_Trajs[i].calculateCoarseParameters();
	}


}

void Trajectories::calcCLset(vector<vector<CL>> &CLset, vector<vector<CTR>> &CTRset)
{
	/*	Calculate the CL(TRi, lj, D)	*/
	cout << "Begin to calculate CL(TRi, lj, D).\n";
	//	Initialize CL for each fine partition
	CLset.resize(m_Trajs.size());

	for (int i = 0; i < (int)m_Trajs.size(); ++i)
	{
		CLset[i].reserve(m_Trajs[i].size() - 1);
		for(int j = 0; j < (int)m_Trajs[i].size() - 1; ++j)
		{
			CL cl;
			CLset[i].push_back(cl);
		}
	}


	vector<vector<Line>> lines;
	lines.reserve(m_Trajs.size());

	for (int i = 0; i < (int)m_Trajs.size(); ++i)
	{
		vector<Line> trajLines;
		trajLines.reserve(m_Trajs[i].size() - 1);
		lines.push_back(trajLines);
		for (int j = 0; j < (int)m_Trajs[i].size() - 1; ++j)
		{
			Line cl;
			cl.first.first = m_Trajs[i][j][0];
			cl.first.second = m_Trajs[i][j][1];
			cl.second.first = m_Trajs[i][j + 1][0];
			cl.second.second = m_Trajs[i][j + 1][1];
			lines[i].push_back(cl);
		}
	}

	vector<vector<pair<int, Line>>> elems;
	elems.reserve(m_Trajs.size());

	for (int i = 0; i < (int)m_Trajs.size(); ++i)
	{
		const vector<int> &CPi = m_Trajs[i].getCoarsePartition();
		vector<pair<int, Line>> trajElem;
		trajElem.reserve(m_Trajs[i].size() - 1);
		elems.push_back(trajElem);
		for (int j = 0; j < (int)CPi.size() - 1; ++j)
		{
			for (int k = CPi[j]; k < CPi[j + 1]; ++k)
			{
				pair<int, Line> elem;
				elem.first = k;
				elem.second = lines[i][k];
				elems[i].push_back(elem);
			}
		}
	}


	for (int i = 0; i < (int)m_Trajs.size(); ++i)
	{
		const vector<int> &CPi = m_Trajs[i].getCoarsePartition();
		for (int k = 0; k < (int)m_Trajs.size(); ++k)
		{
			if(i == k)
				continue;
			const vector<int> &CPk = m_Trajs[k].getCoarsePartition();
			for (int j = 0; j < (int)CPi.size() - 1; ++j)	// the coarse partition of traj i
			{
				for (int l = 0; l < (int)CPk.size() - 1; ++l)
				{
					if(lb(CPi, CPk, i, j, k, l) > m_D)	//	compare Lk and Ll
					{
						continue;
					}
					else	//	partition Li and Lj into fine granularity
					{
						double dist = ub(CPi, CPk, i, j, k, l);
						if(dist <= m_D)	//	Put all the fine partition into CL
						{
							for(int m = CPi[j]; m < CPi[j + 1]; ++m)
							{
								for(int n = CPk[l]; n < CPk[l + 1]; ++n)
								{
									//pair<int, Line> elem;
									//elem.first = k;
									//Line cl;
									//cl.first.first = m_Trajs[k][n][0];
									//cl.first.second = m_Trajs[k][n][1];
									//cl.second.first = m_Trajs[k][n + 1][0];
									//cl.second.second = m_Trajs[k][n + 1][1];
									//elem.second = lines[k][n];

									//pair<int, Line> elem;
									//elem.first = k;
									//elem.second = lines[k][n];
									CLset[i][m].closeLine.push_back(&elems[k][n]);
								}
							}
						}
						else	//	Put part of the fine-partition into CL
						{

							for(int m = CPi[j]; m < CPi[j + 1]; ++m)
							{

								//Line clj;
								//clj.first.first = m_Trajs[i][m][0];
								//clj.first.second = m_Trajs[i][m][1];
								//clj.second.first = m_Trajs[i][m + 1][0];
								//clj.second.second = m_Trajs[i][m + 1][1];

								for(int n = CPk[l]; n < CPk[l + 1]; ++n)
								{
									//pair<int, Line> elem;
									//elem.first = k;
									//Line cll;
									//cll.first.first = m_Trajs[k][n][0];
									//cll.first.second = m_Trajs[k][n][1];
									//cll.second.first = m_Trajs[k][n + 1][0];
									//cll.second.second = m_Trajs[k][n + 1][1];
									//elem.second = lines[k][n];
									//double dist = calculateDistance(clj.first.first, clj.first.second, clj.second.first, clj.second.second,
									//	cll.first.first, cll.first.second, cll.second.first, cll.second.second, m_Wper, m_Wpar, m_Wthe);

									double dist = calculateDistance(lines[i][m].first.first, lines[i][m].first.second, lines[i][m].second.first, lines[i][m].second.second,
										lines[k][n].first.first, lines[k][n].first.second, lines[k][n].second.first, lines[k][n].second.second, m_Wper, m_Wpar, m_Wthe);
									if( dist <= m_D)
									{
										//pair<int, Line> elem;
										//elem.first = k;
										//elem.second = lines[k][n];
										CLset[i][m].closeLine.push_back(&elems[k][n]);
									}
								}
							}
						}
					}
				}
			}
		}
	}

	/*
	//	Output the result
	for(int i = 0; i < (int)m_Trajs.size(); i++)
	{
	vector<CL> tCLset = CLset[i];
	vector<CL>::iterator itr = tCLset.begin();
	int num = 0;
	for (itr; itr != tCLset.end(); itr++)
	{
	cout << "For traj " << i << " and fine partition " << num << ":" << itr->closeLine.size() << endl;
	vector<pair<int, Line>>::iterator itrP = itr->closeLine.begin();
	for (itrP; itrP != itr->closeLine.end(); itrP++)
	{
	cout << "\tFor fine " << num << ":TID,[Line]:" << itrP->first << 
	",[" << itrP->second.first.first << "-" << itrP->second.first.second << "-" 
	<< itrP->second.second.first << "-" << itrP->second.second.second << "]" <<endl;
	}
	num++;
	}
	}
	*/

	cout << "End of calculate CL.\n";

	//	Initialize CTR for each fine partition
	cout << "Begin to calculate the CTR for each fine parititon.\n";

	CTRset.resize(m_Trajs.size());
	for (int i = 0; i < (int)m_Trajs.size(); ++i)
	{
		CTRset.reserve(m_Trajs[i].size() - 1);
		for (int j = 0; j < (int)m_Trajs[i].size() - 1; ++j)
		{
			CTR ctr;
			CTRset[i].push_back(ctr);	//	A CTR set for each partition
		}
	}


	//	Calculate the CTR for each fine partition

	for(int i = 0; i < (int)m_Trajs.size(); ++i)
	{
		for(int j = 0; j < (int)m_Trajs[i].size() - 1; ++j)
		{
			int TID = -1;
			double fParLen = calculateEuclideanDistance(m_Trajs[i][j][0], m_Trajs[i][j][1], m_Trajs[i][j + 1][0], m_Trajs[i][j + 1][1]);
			CL cl = CLset[i][j];	//	The CL set of the partition
			
			double len = 0.0;
			for (int k = 0; k < (int)cl.closeLine.size(); ++k)
			{
				int curTID = cl.closeLine[k]->first;
				if (curTID != TID)	//	a new trajectory
				{
					if (len >= fParLen)
					{
						CTRset[i][j].closeTraj.push_back(TID);
					}
					len = 0.0;
					TID = curTID;
				}
				Line line = CLset[i][j].closeLine[k]->second;	//	get the line of the close fine partition
				double tmpLen = calculateEuclideanDistance(line.first.first, line.first.second, line.second.first, line.second.second);
				len += tmpLen;
			}
			//cout << len << ":" << fParLen << "\t";
			if (len >= fParLen)	//	For the last element of the close line
			{
				CTRset[i][j].closeTraj.push_back(TID);
			} 
		}
	}

	vector<vector<Line>> (lines).swap(lines);
	vector<vector<pair<int, Line>>> (elems).swap(elems);


	/*for (int i = 0; i < (int)m_Trajs.size(); ++i)
	{
		cout << "\tFor traj " << i << endl;
		for (int j = 0; j < (int)m_Trajs[i].size() - 1; ++j)
		{
			cout << "\t\tFor fine partition " << j << " of traj " << i << ", size:" << CTRset[i][j].closeTraj.size() << endl;
		}
	}*/

	cout << "End of the calculating of the CTR.\n";
}

void Trajectories::calcadj(vector<vector<double>> &adj)
{
	// Calculate the standard deviation
	double dOutlierRate = 100.0;
	double dDensityThreash = 0.1;
	double dadjMax = 20.0;
	double dadjMin = 0.05;
	cout << "Begin to calculate the standard deviation.\n";

	int iNumOfPair = 0;
	double dAvgLen = 0.0;

	for (int i = 0; i < (int)m_Trajs.size(); ++i)
	{
		const vector<int>& CPi = m_Trajs[i].getCoarsePartition();
		for (int k = 0; k < (int)m_Trajs.size(); ++k)
		{
			if (i == k)
			{
				continue;
			}
			const vector<int>& CPk = m_Trajs[k].getCoarsePartition();
			for (int j = 0; j < (int)CPi.size() - 1; ++j)
			{					
				Line clj;
				clj.first.first = m_Trajs[i][CPi[j]][0];
				clj.first.second = m_Trajs[i][CPi[j]][1];
				clj.second.first = m_Trajs[i][CPi[j + 1]][0];
				clj.second.second = m_Trajs[i][CPi[j + 1]][1];


				for (int l = 0; l < (int)CPk.size() - 1; ++l)
				{
					Line cll;
					cll.first.first = m_Trajs[k][CPk[l]][0];
					cll.first.second = m_Trajs[k][CPk[l]][1];
					cll.second.first = m_Trajs[k][CPk[l + 1]][0];
					cll.second.second = m_Trajs[k][CPk[l + 1]][1];

					double curLen = calculateDistance(clj.first.first, clj.first.second, clj.second.first, clj.second.second,
						cll.first.first, cll.first.second, cll.second.first, cll.second.second, m_Wper, m_Wpar, m_Wthe);
					if (curLen < dOutlierRate * m_D)
					{
						dAvgLen += curLen;
						++iNumOfPair;
					}
				}
			}
		}
	}

	dAvgLen /= iNumOfPair;
	cout << "Average D is:" << dAvgLen << endl;

	double sd = 0.0;
	for (int i = 0; i < (int)m_Trajs.size(); ++i)
	{
		const vector<int>& CPi = m_Trajs[i].getCoarsePartition();
		for (int k = 0; k < (int)m_Trajs.size(); ++k)
		{
			if (i == k)
			{
				continue;
			}
			const vector<int>& CPk = m_Trajs[k].getCoarsePartition();
			for (int j = 0; j < (int)CPi.size() - 1; ++j)
			{					
				Line clj;
				clj.first.first = m_Trajs[i][CPi[j]][0];
				clj.first.second = m_Trajs[i][CPi[j]][1];
				clj.second.first = m_Trajs[i][CPi[j + 1]][0];
				clj.second.second = m_Trajs[i][CPi[j + 1]][1];


				for (int l = 0; l < (int)CPk.size() - 1; ++l)
				{
					Line cll;
					cll.first.first = m_Trajs[k][CPk[l]][0];
					cll.first.second = m_Trajs[k][CPk[l]][1];
					cll.second.first = m_Trajs[k][CPk[l + 1]][0];
					cll.second.second = m_Trajs[k][CPk[l + 1]][1];

					double dist = calculateDistance(clj.first.first, clj.first.second, clj.second.first, clj.second.second,
						cll.first.first, cll.first.second, cll.second.first, cll.second.second, m_Wper, m_Wpar, m_Wthe);
					if (dist < dOutlierRate * m_D)
					{
						sd += pow(dist - dAvgLen, 2);
					}

				}
			}
		}
	}

	sd = sqrt(sd / iNumOfPair);
	cout << "\tStandard deviation is:" << sd << endl;

	cout << "End of calculating the standard deviation.\n";



	//	Calculate the CL(TRi, lj, sd)

	cout << "Begin to calculate the CL(TRi, lj, sd).\n";

	vector<vector<int>> sdCLset;
	sdCLset.resize(m_Trajs.size());

	for (int i = 0; i < (int)m_Trajs.size(); ++i)
	{
		sdCLset[i].resize(m_Trajs[i].size() - 1);
	}

	for (int i = 0; i < (int)m_Trajs.size(); ++i)
	{
		const vector<int>& CPi = m_Trajs[i].getCoarsePartition();
		for (int k = 0; k < (int)m_Trajs.size(); ++k)
		{
			if(i == k)
				continue;
			const vector<int>& CPk = m_Trajs[k].getCoarsePartition();
			for (int j = 0; j < (int)m_Trajs[i].getCoarsePartition().size() - 1; ++j)	// the coarse partition of traj i
			{
				for (int l = 0; l < (int)CPk.size() - 1; ++l)
				{
					if(lb(CPi, CPk, i, j, k, l) > m_D)	//	compare Lk and Ll
					{
						continue;
					}
					else
					{
						if(ub(CPi, CPk, i, j, k, l) <= sd)
						{
							for(int m = CPi[j]; m < CPi[j + 1]; ++m)
							{
								for(int n = CPk[l]; n < CPk[l + 1]; ++n)
								{
									pair<int, Line> elem;
									elem.first = k;
									Line cl;
									cl.first.first = m_Trajs[k][n][0];
									cl.first.second = m_Trajs[k][n][1];
									cl.second.first = m_Trajs[k][n + 1][0];
									cl.second.second = m_Trajs[k][n + 1][1];
									elem.second = cl;
									++sdCLset[i][m];
								}
							}
						}
						else
						{
							Line clj;
							clj.first.first = m_Trajs[i][CPi[j]][0];
							clj.first.second = m_Trajs[i][CPi[j]][1];
							clj.second.first = m_Trajs[i][CPi[j + 1]][0];
							clj.second.second = m_Trajs[i][CPi[j + 1]][1];

							for(int m = CPi[j]; m < CPi[j + 1]; ++m)
							{
								for(int n = CPk[l]; n < CPk[l + 1]; ++n)
								{
									pair<int, Line> elem;
									elem.first = k;
									Line cll;
									cll.first.first = m_Trajs[k][n][0];
									cll.first.second = m_Trajs[k][n][1];
									cll.second.first = m_Trajs[k][n + 1][0];
									cll.second.second = m_Trajs[k][n + 1][1];
									elem.second = cll;
									if(calculateDistance(clj.first.first, clj.first.second, clj.second.first, clj.second.second,
										cll.first.first, cll.first.second, cll.second.first, cll.second.second, m_Wper, m_Wpar, m_Wthe) <= sd)
									{
										++sdCLset[i][m];
									}
								}
							}
						}
					}
				}
			}
		}
	}

	/*for (int i = 0; i < (int)m_Trajs.size(); ++i)
	{
		cout << "\tFor traj " << i << ":" << endl;
		for (int j = 0; j < (int)sdCLset[i].size(); ++j)
		{
			cout << "\t\tFor partition " << j << " the size is:" << sdCLset[i][j] << endl;
		}
	}*/

	cout << "End of calculating CL(TRi, lj, sd).\n";



	cout << "Begin to calculate adj(li).\n";

	int iL = 0;
	vector<vector<int>>::iterator itrsdSetCL = sdCLset.begin();

	for (itrsdSetCL; itrsdSetCL != sdCLset.end(); ++itrsdSetCL)
	{
		for (int i = 0; i < (int)itrsdSetCL->size(); ++i)
		{
			if (sdCLset[i].size() != 0)
			{
				++iL;
			}
		}
		//iL += itrsdSetCL->size();
	}

	cout << "\tThe cardinal of iL is:" << iL << endl;

	int iSumDensity = 0;

	for (int i = 0; i < (int)m_Trajs.size(); ++i)
	{
		for (int j = 0; j < (int)sdCLset[i].size(); ++j)
		{
			iSumDensity += sdCLset[i].size();
		}
	}

	cout << "\tThe sum of the density is:" << iSumDensity << endl;

	double dNumerator = static_cast<double>(iSumDensity) / iL;

	adj.resize(m_Trajs.size());
	for (int i = 0; i < (int)m_Trajs.size(); ++i)
	{ 
		adj[i].resize(m_Trajs[i].size() - 1);
		//cout << "\tFor traj " << i << endl;
		for (int j = 0; j < (int)m_Trajs[i].size() - 1; ++j)
		{

			adj[i][j] = dNumerator / sdCLset[i][j];

			if (adj[i][j] < dadjMin)
			{
				adj[i][j] = dadjMin;
			}
			if (adj[i][j] > dadjMax)
			{
				adj[i][j] = dadjMax;
			}
			//cout << "\t\tFor partition " << j << ", the adj is:" << adj[i][j] << endl;
		}
	}

	cout << "End of calculating adj(li).\n";

	sdCLset.clear();
	vector<vector<int>> (sdCLset).swap(sdCLset);
}

void Trajectories::calcOP(vector<vector<int>> &OP, const vector<vector<CTR>> &CTRset, const vector<vector<double>> &adj)
{

	cout << "Begin to calculate OP(TRi, D, p).\n";
	OP.resize(m_Trajs.size());

	double iThresh = ceil((1 - m_p) * m_Trajs.size());

	for (int i = 0; i < (int)m_Trajs.size(); ++i)
	{
		for (int j = 0; j < (int)m_Trajs[i].size() - 1; ++j)
		{
			double rate = ceil(CTRset[i][j].closeTraj.size() * adj[i][j]);
			if ( rate < iThresh)
			{
				OP[i].push_back(j);	// store the id of the fine partition
				//cout << "\t\tIn traj " << i << ", partition " << j << "rate,iThread:" << rate << "," << iThresh << endl;
			}
		}		
	}

	/*for (int i = 0; i < (int)m_Trajs.size(); i++)
	{
		cout << " For traj " << i << ", there are " << OP[i].size() << " outlier partition out of " << m_Trajs[i].size() - 1 << " partitions.\n";
	}*/

	cout << "End of calculating OP(TRi, D, p).\n";
}

void Trajectories::calcOT(const vector<vector<int>> &OP)
{
	cout << "Begin to calculate the OT.\n";

	int iOutlier = 0;

	for (int i = 0; i < (int)m_Trajs.size(); ++i)
	{
		double dDominate = 0.0;
		double dNumerator = 0.0;

		for (int j = 0; j < (int)OP[i].size(); ++j)
		{
			double x0, x1, y0, y1;
			x0 = m_Trajs[i][OP[i][j]][0];
			y0 = m_Trajs[i][OP[i][j]][1];
			x1 = m_Trajs[i][OP[i][j] + 1][0];
			y1 = m_Trajs[i][OP[i][j] + 1][1];
			dNumerator += calculateEuclideanDistance(x0, y0, x1, y1);
		}

		for (int j = 0; j < (int)m_Trajs[i].size() - 1; ++j)
		{
			double x0, x1, y0, y1;
			x0 = m_Trajs[i][j][0];
			y0 = m_Trajs[i][j][1];
			x1 = m_Trajs[i][j + 1][0];
			y1 = m_Trajs[i][j + 1][1];
			dDominate += calculateEuclideanDistance(x0, y0, x1, y1);
		}

		if ((dNumerator / dDominate) >= m_F)
		{
			//cout << "\t\tFor traj " << i << ":" << dNumerator / dDominate << ", dDominate:" << dDominate << endl;
			m_OT.push_back(i);
			++iOutlier;
		}
	}


	//vector<int>::iterator itrOT = m_OT.begin();

	cout << "There are " << iOutlier << " outlier out of " << m_Trajs.size() << " trajectories\n";

	/*for (itrOT; itrOT != m_OT.end(); itrOT++)
	{
	cout << *itrOT << "\t";
	}*/

	cout << "\nEnd of calculating the OT.\n";

}

void Trajectories::writeResult(const string& outputFileName) const
{
	ofstream outStream(outputFileName.c_str(), ios::app);

	outStream << endl << endl;
	outStream << "Parameters:" << m_Wper << "\t" << m_Wpar << "\t" << m_Wthe << "\t" << m_D << "\t" << m_p << "\t" << m_F << endl;
	outStream << "\t" << m_OT.size() << " outlier out of " << m_Trajs.size() << " trajectories.\tTime cost:" << costTime << endl;
	vector<int>::const_iterator itrOT = m_OT.begin();
	for (itrOT; itrOT != m_OT.end(); itrOT++)
	{
		outStream << *itrOT << "\t";
	}

	outStream.close();
}

_TRAJECTORY_END_